import rospy
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64


class ReBroadcast:
    def spawn_pub(self,key):
        pub = rospy.Publisher("/{}/joint_{}_position_controller/command".format(self.ROBOT,key),Float64,queue_size=10)
        return pub

    def __init__(self):

        rospy.init_node("ReBroadcast")
        self.ROBOT = rospy.get_param("~robot")
        self.publishers = {}
        self.keys = ["spin","arm1","arm2","arm3","finger1","finger2"]
        for key in self.keys:
            pub = self.spawn_pub(key)
            self.publishers[key] = pub

        rospy.Subscriber("/joint_states",JointState,self.call_back)
        rospy.spin()
    
    def call_back(self,data):
        msg = Float64()
        state = {}
        for key,value in zip(data.name, data.position):
            key=key.replace("_joint","")
            state[key] = value
        
        for key in self.keys:
            msg.data = state[key]
            self.publishers[key].publish(msg)


if __name__ == "__main__":
    controller = ReBroadcast()